/*
参考uarm metal的逆向结算代码，驱动舵机版的机械臂，可以正常使用
*/

#include "ik.h"
#include <Servo.h>

int STEP = 10;//5;
int DELAY = 10;//25;
int REDRESS_RIGHT = 80.0;

Servo servo_base, servo_left, servo_right, air, lock;

double x, y, z;
double targetRot = 0;
double targetLeft = 0;
double targetRight = 0;

double curr_x = 0, curr_y = 0, curr_z = 0;

int pinInterruptStop = 2;//电磁接触传感器触发
int pinInterrupt = 3; //接中断信号的脚 

bool isRunning = false;
bool isTrigger = false;

double redress_right_servo(double angle) {
    return angle + REDRESS_RIGHT;
}


void suck_on() {
  air.write(180);
  lock_off();
}

void suck_keep() {
  suck_off();
  lock_on();
}

void suck_off() {
  air.write(0);
  lock_on();
}

void lock_on() {
  lock.write(180);
}

void lock_off() {
  lock.write(0);
}


void setup() {
    Serial.begin(9600);
    // for (int iz = 50; iz < 100; iz ++) {
    //     xyzToAngle(0, 250, iz, targetRot, targetLeft, targetRight, false);
    //     Serial.print(targetRot);
    //     Serial.print("\t");
    //     Serial.print(targetLeft);
    //     Serial.print("\t");
    //     Serial.print(targetRight);
    //     Serial.print("\t\n");
    // }
    servo_base.attach(A0);
    servo_left.attach(A2);
    servo_right.attach(A1);
    air.attach(A4);
    lock.attach(A5);

// move(200, 100, 200, 0);
    curr_x = 200;
    curr_y = 100;
    curr_z = 200;
    xyzToAngle(curr_x, curr_y, curr_z, targetRot, targetLeft, targetRight, false);
    servo_base.write(targetRot);
    servo_left.write(targetLeft);
    servo_right.write(redress_right_servo(targetRight));

    pinMode( pinInterruptStop, INPUT_PULLUP);//设置管脚为输入      //Enable中断管脚, 中断服务程序为onChange(), 监视引脚变化   
    pinMode( pinInterrupt, INPUT);//设置管脚为输入      //Enable中断管脚, 中断服务程序为onChange(), 监视引脚变化   
    attachInterrupt( digitalPinToInterrupt(pinInterrupt), onRising, RISING);
//    action();
}

void action() {
  
    suck_off();
//    move(0, 200, 200, 0);
//    move(100, 200, 200, 0);
    move(200, 100, 200, 0);

    move(200, 100, 60, pinInterruptStop);
    suck_on();
    delay(1000);
//    suck_keep();
    move(200, 100, 200, 0);
    move(100, 200, 200, 0);
    move(0, 200, 200, 0);
    move(0, 200, 60, 0);
    suck_off();
    move(0, 200, 200, 0);
    move(0, 200, 200, 0);
    move(200, 100, 200, 0);
}

void onRising()
{ 
  isTrigger = true;
}

void move(int x_point, int y_point, int z_point, int pin) {
    double x_step, y_step, z_step;

    //确定每个轴的运行方向  正负
    x_step = x_point > curr_x ? 1 : (-1);
    y_step = y_point > curr_y ? 1 : (-1);
    z_step = z_point > curr_z ? 1 : (-1);

//    Serial.println(z_step);

    //选一个最大差值的轴
    int delta_x = abs(x_point - curr_x);
    int delta_y = abs(y_point - curr_y);
    int delta_z = abs(z_point - curr_z);

//    Serial.println(delta_z);

    int max_delta = delta_x > delta_y ? delta_x : delta_y;
    max_delta = max_delta > delta_z ? max_delta : delta_z;

//    Serial.println(max_delta);

    double delta_x_step = ((double)delta_x) / ((double)max_delta) * x_step;
    double delta_y_step = ((double)delta_y) / ((double)max_delta) * y_step;
    double delta_z_step = ((double)delta_z) / ((double)max_delta) * z_step;

//    Serial.println(delta_z_step);

    while(max_delta > 0) {
        if (pin) {
          if (digitalRead(pin) == LOW) {
            delay(100);
            if (digitalRead(pin) == LOW) {
              break;
            }
          }
        }

        curr_x += delta_x_step;
        curr_y += delta_y_step;
        curr_z += delta_z_step;
//        Serial.println(curr_x);
//        Serial.println(curr_y);
//        Serial.println(curr_z);
//        Serial.println();
        xyzToAngle(curr_x, curr_y, curr_z, targetRot, targetLeft, targetRight, false);
        servo_base.write(targetRot);
        servo_left.write(targetLeft);
        servo_right.write(redress_right_servo(targetRight));
        delay(DELAY);
        max_delta --;
    }
}

void loop() {
  if (isTrigger && !isRunning) {
    if (digitalRead(pinInterrupt) == HIGH) {
      isRunning = true;
      action();
      isRunning = false;
      isTrigger = false;
    } else {
      isTrigger = false;
      return;
    }
    
  }
}
